#ifndef NAV_CONTROLLER_TEST_HPP
#define NAV_CONTROLLER_TEST_HPP

#include "rclcpp/rclcpp.hpp"
#include "nav2_core/controller.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/robot_utils.hpp"

#include "nav2_msgs/msg/costmap.hpp"

namespace nav_controller_test
{
    class NavControllerTest : public nav2_core::Controller
    {
    private:
        nav2_util::LifecycleNode::SharedPtr node_;
        std::string name_;
        std::shared_ptr<tf2_ros::Buffer> buffer_;
        std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
        nav_msgs::msg::Path path_;

        double max_linear_speed_, max_angular_speed_;
        // 获取路径中距离当前点最近的点
        geometry_msgs::msg::PoseStamped getNearTargetPose(const geometry_msgs::msg::PoseStamped &current_pose);
        // 计算目标点方向和当前朝向的角度差
        double calculateAngleDifference(const geometry_msgs::msg::PoseStamped &current_pose,
                                        const geometry_msgs::msg::PoseStamped &target_pose);

    public:
        NavControllerTest() = default;
        ~NavControllerTest() = default;

        void configure(
            const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent,
            std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
            std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;

        void cleanup() override;

        void activate() override;

        void deactivate() override;

        void setPlan(const nav_msgs::msg::Path &path) override;

        geometry_msgs::msg::TwistStamped computeVelocityCommands(
            const geometry_msgs::msg::PoseStamped &pose,
            const geometry_msgs::msg::Twist &velocity,
            nav2_core::GoalChecker *goal_checker) override;

        void setSpeedLimit(const double &speed_limit, const bool &percentage) override;
    };

} // namespace nav_controller_test

#endif